#include "moveit/rviz_plugin_scene_editor/scene_editor_display.h"
#include "moveit/rviz_plugin_scene_editor/scene_editor_frame.h"

#include <moveit/robot_interaction/kinematic_options_map.h>

#include <rviz/visualization_manager.h>
#include <rviz/robot/robot_link.h>

#include <rviz/properties/property.h>
#include <rviz/properties/string_property.h>
#include <rviz/properties/float_property.h>
#include <rviz/properties/ros_topic_property.h>
#include <rviz/properties/editable_enum_property.h>
#include <rviz/properties/color_property.h>
#include <rviz/display_context.h>
#include <rviz/frame_manager.h>
#include <rviz/window_manager_interface.h>
#include <rviz/display_factory.h>
#include <rviz/ogre_helpers/shape.h>

#include <QShortcut>
#include <QProgressBar>

#include "ui_rviz_plugin_scene_editor_frame.h"

namespace moveit_rviz_plugin_scene_editor
{
SceneEditorDisplay::SceneEditorDisplay()
  : SceneEditorDisplayHelper(), frame_(nullptr), frame_dock_(nullptr) /*, int_marker_display_(nullptr)*/
{
  // Start background jobs
  background_process_.setJobUpdateEvent(boost::bind(&SceneEditorDisplay::backgroundJobUpdate, this, _1, _2));
}

// ******************************************************************************************
// Deconstructor
// ******************************************************************************************
SceneEditorDisplay::~SceneEditorDisplay()
{
  background_process_.clearJobUpdateEvent();
  clearJobs();

  if (frame_dock_)
    delete frame_dock_;
}

void SceneEditorDisplay::load(const rviz::Config& config)
{
  SceneEditorDisplayHelper::load(config);
}

void SceneEditorDisplay::save(rviz::Config config) const
{
  SceneEditorDisplayHelper::save(config);
}

void SceneEditorDisplay::update(float wall_dt, float ros_dt)
{
  if (frame_)
    frame_->updateSceneMarkers(wall_dt, ros_dt);

  SceneEditorDisplayHelper::update(wall_dt, ros_dt);
}

void SceneEditorDisplay::reset()
{
  SceneEditorDisplayHelper::reset();

  bool enabled = this->isEnabled();
  frame_->disable();
  if (enabled)
  {
    frame_->enable();
  }
}

void SceneEditorDisplay::onInitialize()
{
  SceneEditorDisplayHelper::onInitialize();

  rviz::WindowManagerInterface* window_context = context_->getWindowManager();
  frame_ = new SceneEditorFrame(this, context_, window_context ? window_context->getParentWindow() : nullptr);

  if (window_context)
  {
    frame_dock_ = window_context->addPane(getName(), frame_);
    connect(frame_dock_, SIGNAL(visibilityChanged(bool)), this, SLOT(motionPanelVisibilityChange(bool)));
    frame_dock_->setIcon(getIcon());
    frame_dock_->setWindowTitle("场景编辑");
  }
}

void SceneEditorDisplay::onEnable()
{
  SceneEditorDisplayHelper::onEnable();

  frame_->enable();
}

void SceneEditorDisplay::onDisable()
{
  SceneEditorDisplayHelper::onDisable();

  frame_->disable();
}

void SceneEditorDisplay::fixedFrameChanged()
{
  SceneEditorDisplayHelper::fixedFrameChanged();
}

void SceneEditorDisplay::updateInternal(float wall_dt, float ros_dt)
{
  SceneEditorDisplayHelper::updateInternal(wall_dt, ros_dt);
}

void SceneEditorDisplay::onSceneMonitorReceivedUpdate(
    planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
{
  SceneEditorDisplayHelper::onSceneMonitorReceivedUpdate(update_type);
  if (frame_)
    frame_->sceneUpdate(update_type);
}

void SceneEditorDisplay::motionPanelVisibilityChange(bool enable)
{
  if (enable)
    setEnabled(true);
}

void SceneEditorDisplay::backgroundJobUpdate(moveit::tools::BackgroundProcessing::JobEvent /*unused*/,
                                             const std::string& /*unused*/)
{
  addMainLoopJob(boost::bind(&SceneEditorDisplay::updateBackgroundJobProgressBar, this));
}

void SceneEditorDisplay::updateBackgroundJobProgressBar()
{
  if (!frame_)
    return;
  QProgressBar* p = frame_->ui_->background_job_progress;
  int n = background_process_.getJobCount();

  if (n == 0)
  {
    p->setValue(p->maximum());
    p->update();
    p->hide();
    p->setMaximum(0);
  }
  else
  {
    if (n == 1)
    {
      if (p->maximum() == 0)
        p->setValue(0);
      else
        p->setValue(p->maximum() - 1);
    }
    else
    {
      if (p->maximum() < n)
        p->setMaximum(n);
      else
        p->setValue(p->maximum() - n);
    }
    p->show();
    p->update();
  }
}

void SceneEditorDisplay::onRobotModelLoaded()
{
  SceneEditorDisplayHelper::onRobotModelLoaded();
}

// Pick and place
void SceneEditorDisplay::clearPlaceLocationsDisplay()
{
  for (std::shared_ptr<rviz::Shape>& place_location_shape : place_locations_display_)
    place_location_shape.reset();
  place_locations_display_.clear();
}

void SceneEditorDisplay::visualizePlaceLocations(const std::vector<geometry_msgs::PoseStamped>& place_poses)
{
  clearPlaceLocationsDisplay();
  place_locations_display_.resize(place_poses.size());
  for (std::size_t i = 0; i < place_poses.size(); ++i)
  {
    place_locations_display_[i].reset(new rviz::Shape(rviz::Shape::Sphere, context_->getSceneManager()));
    place_locations_display_[i]->setColor(1.0f, 0.0f, 0.0f, 0.3f);
    Ogre::Vector3 center(place_poses[i].pose.position.x, place_poses[i].pose.position.y, place_poses[i].pose.position.z);
    Ogre::Vector3 extents(0.02, 0.02, 0.02);
    place_locations_display_[i]->setScale(extents);
    place_locations_display_[i]->setPosition(center);
  }
}

}  // namespace moveit_rviz_plugin_scene_editor
